use crate::controller::aeb::interface::AEBController;
use crate::controller::aeb::interface::AEBControllerInterface;
use super::ttcCalculation::TTCCalculation;
use super::stoppingCalculation::StoppingTimeCalculation;

enum Status {
    DEFAULT,
    FCW,
    BREAKING1,
    BREAKING2,
    FULL_BREAKING
}



pub struct AEBControllerImpl{
    aeb:AEBController,
    ttc_calc:TTCCalculation,
    stopping_time_calc:StoppingTimeCalculation,
    pb1_decel:f64,
    pb2_decel:f64,
    fb_decel:f64,
    status:Status
}



impl AEBControllerInterface for AEBControllerImpl {
    fn controller(
        &mut self,
        ego_velocity:f64,
        mio_distance:f64,
        mio_velocity:f64
    )->(bool,bool,bool,i8,f64) {
        let (collision,ttc)=self.ttcCalculation(mio_distance, mio_velocity);
        let (fcw_stopping_time,pb1_stopping_time,pb2_stopping_time,fb_stopping_time)=self.stoppingTimeCalculation(ego_velocity);
        let stop=ego_velocity<=0.1;
        let (fcw_activate,aeb_status,deceleration)=self.aebLogic(ttc, fcw_stopping_time,pb1_stopping_time,pb2_stopping_time, fb_stopping_time, self.pb1_decel, self.pb2_decel, self.fb_decel, stop);

         /// collision,egoCarStop,FCWactivate,AEBstatus,deceleration
        (collision,stop,fcw_activate,aeb_status,deceleration) 


    }
}
impl AEBControllerImpl {
    pub fn new(
        pb1_decel:f64,
        pb2_decel:f64,
        fb_decel:f64,
        ttc_calc:TTCCalculation,
        stopping_time_calc:StoppingTimeCalculation,
    ) -> AEBControllerImpl{
        AEBControllerImpl{
            status:Status::DEFAULT,
            ttc_calc:ttc_calc,
            stopping_time_calc:stopping_time_calc,
            aeb:AEBController {},
            pb1_decel:pb1_decel,
            pb2_decel:pb2_decel,
            fb_decel:fb_decel,
        }
    }

    ///
    ///ttc calculation 
    /// return (collision,ttc) 
    fn ttcCalculation(
        &self,
        mio_distance:f64,
        mio_velocity:f64
    )->(bool,f64){
        self.ttc_calc.calculation(mio_distance, mio_velocity)
    }

    /// clac time threshold
    /// return (fcw_stopping_time,pb1_stopping_time,pb2_stopping_time,fb_stopping_time)
    fn stoppingTimeCalculation(
        &self,
        ego_velocity:f64,
    )->(f64,f64,f64,f64){
        self.stopping_time_calc.calculation(ego_velocity, self.pb1_decel, self.pb2_decel, self.fb_decel)


    }


    

    ///this is aeb logic
    /// return (fcw_activate,aeb_status,deceleration)
    fn aebLogic(
        &mut self,
        ttc:f64,
        fcw_time:f64,
        pb1_time:f64,
        pb2_time:f64,
        fb_time:f64,
        pb1_decel:f64,
        pb2_decel:f64,
        fb_decel:f64,
        stop:bool
    )->(bool,i8,f64){
        //init
        let aeb_status=false;
        let fcw_activate:i8=0;
        let decel=0.0;
        //写作ifelse 或者 流式调用 考虑到先写逻辑 先不做流式
        //但是从各种意义上都是流式比较好


        let matchFuntion=|status:&Status|->(bool,i8,f64){
            match status {
                Status::DEFAULT => (false,0,0.0),
                Status::FCW => (true,0,0.0),
                Status::BREAKING1 => (true,1,pb1_decel),
                Status::BREAKING2 => (true,2,pb2_decel),
                Status::FULL_BREAKING => (true,3,fb_decel),
            }
        };


        match self.status {
            Status::DEFAULT => {
                if ttc.abs()<fcw_time && ttc<0.0{
                    self.status=Status::FCW;
                }
            },
            Status::FCW => {
                if ttc.abs()<pb1_time && ttc<0.0{
                    self.status=Status::BREAKING1;
                }
                else if ttc.abs()>=1.2*fcw_time{
                    self.status=Status::DEFAULT;
                }
                
            },
            Status::BREAKING1 => {
                if ttc.abs()<pb2_time && ttc<0.0{
                    self.status=Status::BREAKING2;
                }
                else if stop==true{
                    self.status=Status::DEFAULT;
                }
            },
            Status::BREAKING2 => {
                if ttc.abs()<fb_time && ttc<0.0{
                    self.status=Status::FULL_BREAKING;
                }
                else if stop==true{
                    self.status=Status::DEFAULT;
                }
            },
            Status::FULL_BREAKING => {
               if stop==true{
                    self.status=Status::DEFAULT;
                }
            },
        }
        return matchFuntion(&self.status);



    }

}


